/*
 * $Id: spm_matfuns.c 247 2005-10-04 17:20:34Z guillaume $
 */

#include <math.h>

int gaussj(double *a, int n, double *b, int m)
{
	int icol = 0, ipiv[64], irow = 0, i, j, k, l, indxc[64], indxr[64], ll;
	double pivinv, big, dum;

	if (n>64) return(1);

	for (j=0; j<n; ++j)
		ipiv[j] = 0;
	for (i=0; i<n; ++i)
	{
		big = 0.0;
		for (j=0; j<n; ++j)
			if (ipiv[j] != 1)
				for (k=0; k<n; ++k)
				{
					if (ipiv[k] == 0)
					{
						if (fabs(a[j + k * n]) >= big)
						{
							big  = fabs(a[j + k * n]);
							irow = j;
							icol = k;
						}
					}
					else if (ipiv[k] > 1)
						return(1);
				}
		++ipiv[icol];
		if (irow != icol)
		{
			for (l=0; l<n; ++l)
			{
				dum = a[irow + l * n];
				a[irow + l * n] = a[icol + l * n];
				a[icol + l * n] = dum;
			}
			for (l=0; l<m; ++l)
			{
				dum = b[irow + l * n];
				b[irow + l * n] = b[icol + l * n];
				b[icol + l * n] = dum;
			}
		}
		indxr[i] = irow;
		indxc[i] = icol;
		if (a[icol + icol * n] == 0.0)
			return(1);
		pivinv = 1.0 / a[icol + icol * n];
		a[icol + icol * n] = 1.0;
		for (l=0; l<n; ++l)
			a[icol + l * n] *= pivinv;
		for (l=0; l<m; ++l)
			b[icol + l * n] *= pivinv;
		for (ll=0; ll<n; ++ll)
			if (ll != icol)
			{
				dum = a[ll + icol * n];
				a[ll + icol * n] = 0.0;
				for (l=0; l<n; ++l)
					a[ll + l * n] -= a[icol + l * n] * dum;
				for (l=0; l<m; ++l)
					b[ll + l * n] -= b[icol + l * n] * dum;
			}
	}
	for (l=n-1; l>=0; --l)
		if (indxr[l] != indxc[l])
			for (k=0; k<n; ++k)
			{
				dum = a[k + indxr[l] * n];
				a[k + indxr[l] * n] = a[k + indxc[l] * n];
				a[k + indxc[l] * n] = dum;
			}
	return(0);
}

/* M = MG\MF */
int AbackslashB(double *MG, double *MF, double *M)
{
	int i;
	double IMG[16];
	for(i=0; i<16; i++)
	{
		IMG[i] = MG[i];
		M[i]   = MF[i];
	}
	if (gaussj(IMG, 4, M, 4)) return(1);
	return(0);
}

void MtimesX(double *M, double *x, double *y)
{
	y[0] = M[0+4*0]*x[0] + M[0+4*1]*x[1] + M[0+4*2]*x[2] + M[0+4*3];
	y[1] = M[1+4*0]*x[0] + M[1+4*1]*x[1] + M[1+4*2]*x[2] + M[1+4*3];
	y[2] = M[2+4*0]*x[0] + M[2+4*1]*x[1] + M[2+4*2]*x[2] + M[2+4*3];
}


/*
void mexFunction(int nlhs, Matrix *plhs[], int nrhs, Matrix *prhs[])
{
	unsigned int n, m, i;
	double *MG, *MF, *M, tmp[16];

	if (nrhs == 0) mexErrMsgTxt("usage: M=leftdiv(MG,MF)");
	if (nrhs != 2) mexErrMsgTxt("leftdiv: 2 input arguments required");
	if (nlhs > 1) mexErrMsgTxt("leftdiv: only 1 output argument required");

	if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) || !mxIsFull(prhs[0]) || !mxIsDouble(prhs[0]))
		mexErrMsgTxt("leftdiv: MG must be numeric, real, full and double");
	if (!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !mxIsFull(prhs[1]) || !mxIsDouble(prhs[1]))
		mexErrMsgTxt("leftdiv: MF must be numeric, real, full and double");

	if (mxGetM(prhs[0])!=4 || mxGetN(prhs[0])!=4 || mxGetM(prhs[1])!=4 || mxGetN(prhs[1])!=4)
		mexErrMsgTxt("leftdiv: all matrices must be 4x4");
	MG = mxGetPr(prhs[0]);
	MF = mxGetPr(prhs[1]);

	plhs[0] = mxCreateFull(4,4, REAL);
	M = mxGetPr(plhs[0]);

	AbackslashB(MG, MF, M);
}

void mexFunction(int nlhs, Matrix *plhs[], int nrhs, Matrix *prhs[])
{
	unsigned int n, m, i;
	double *X, *A, *B, *IA;

	if (nrhs == 0) mexErrMsgTxt("usage: [X, IB]=leftdiv(B,C)");
	if (nrhs != 2) mexErrMsgTxt("leftdiv: 2 input arguments required");
	if (nlhs > 2) mexErrMsgTxt("leftdiv: only 2 output arguments required");

	if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) || !mxIsFull(prhs[0]) || !mxIsDouble(prhs[0]))
		mexErrMsgTxt("leftdiv: A must be numeric, real, full and double");
	A = mxGetPr(prhs[0]);
	n = mxGetM(prhs[0]);
	if (mxGetN(prhs[0]) != n)
		mexErrMsgTxt("leftdiv: A has incompatible no of columns");
	if (!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !mxIsFull(prhs[1]) || !mxIsDouble(prhs[1]))
		mexErrMsgTxt("leftdiv: B must be numeric, real, full and double");
	B = mxGetPr(prhs[1]);
	if (mxGetM(prhs[1]) != n)
		mexErrMsgTxt("leftdiv: B has incompatible m dimension");
	m = mxGetN(prhs[1]);

	plhs[0] = mxCreateFull(n,m, REAL);
	X = mxGetPr(plhs[0]);

	plhs[1] = mxCreateFull(n,n, REAL);
	IA = mxGetPr(plhs[1]);

	for(i=0; i<m*n; i++) X[i]  = B[i];
	for(i=0; i<n*n; i++) IA[i] = A[i];

	(void)gaussj(IA,m,X,n);
}
*/
